/**
 * @file /src/main_window.cpp
 *
 * @brief Implementation for the qt gui.
 *
 * @date 2021.7.21
 **/
/*****************************************************************************
** Includes
*****************************************************************************/

#include <QtGui>
#include <QMessageBox>
#include <iostream>
#include "../include/weeding_robot_gui/main_window.hpp"
#include <time.h>
#include <stdio.h>
#include <stdlib.h>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace weeding_robot_gui {

using namespace Qt;

/*****************************************************************************
** Implementation [MainWindow]
*****************************************************************************/

MainWindow::MainWindow(int argc, char** argv, QWidget *parent)
  : QMainWindow(parent)
  , qnode(argc,argv)
{
  ui.setupUi(this); // Calling this incidentally connects all ui's triggers to on_...() callbacks in this class.

  /* 终端命令输入 */
  cmd = new QProcess(this);
  cmd->start("bash"); //启动终端(Windows下改为cmd)
  cmd->waitForStarted(); //等待启动完成

  QObject::connect(ui.actionAbout_Qt, SIGNAL(triggered(bool)), qApp, SLOT(aboutQt())); // qApp is a global variable for the application
  initUI();
  ReadSettings();
  setWindowIcon(QIcon(":/images/icon.png"));
  ui.tabWidget_Display_Panel->setCurrentWidget(ui.table_Robot_Status);
  ui.tabWidget_Control_Panel->setCurrentWidget(ui.tab_robot_control);
  //ui.tab_manager->setCurrentIndex(0); // ensure the first tab is showing - qt-designer should have this already hardwired, but often loses it (settings?).
  QObject::connect(&qnode, SIGNAL(rosShutdown()), this, SLOT(close()));
  //ui.dock_status->show();

  connections();
  /*********************
    ** Logging
    **********************/
  ui.listView_Logging->setModel(qnode.loggingModel());
  QObject::connect(&qnode, SIGNAL(loggingUpdated()), this, SLOT(updateLoggingView()));

  /*********************
    ** Auto Start
    **********************/
  if ( ui.checkbox_remember_settings->isChecked() ) {
    on_button_connect_clicked(true);
  }
  initSerialPort();
}

MainWindow::~MainWindow()
{
  delete setting;
  delete robot_rocker;
  delete actuator_rocker;
  delete linear_velocity_display_dashboard;
  delete angle_velocity_display_dashboard;
  delete current_serial_port;
  delete timerSerial;
  if(cmd->Running)
  {
    cmd->terminate();
    cmd->waitForFinished();
  }
  delete cmd;
}

/*****************************************************************************
** My Implementation [Functions]
*****************************************************************************/
void MainWindow::initUI()
{
  g_x = 0;
  g_y = 0;
  g_z = 0;

  linear_velocity_display_dashboard = new CCtrlDashBoard(ui.widget_dashboard_linearVelocity);
  linear_velocity_display_dashboard->setGeometry(ui.widget_dashboard_linearVelocity->rect());
  linear_velocity_display_dashboard->setValue(0);

  angle_velocity_display_dashboard = new CCtrlDashBoard(ui.widget_dashboard_angleVelocity);
  angle_velocity_display_dashboard->setGeometry(ui.widget_dashboard_angleVelocity->rect());
  angle_velocity_display_dashboard->setValue(0);

  robot_rocker = new rocker(ui.widget_steering_wheel_rocker);
  actuator_rocker = new rocker(ui.widget_actuator_rocker);
  ui.checkBox_robot_control_used->setEnabled(false);
  ui.checkBox_robot_control_used->setChecked(false);
  ui.checkBox_camera_control_used->setChecked(false);
  ui.checkBox_actuator_control_used->setChecked(false);
  ui.checkBox_actuator_control_used->setEnabled(false);
  ui.pushButton_camera_upforward->setEnabled(camera_control_enabled);
  ui.pushButton_camera_downforward->setEnabled(camera_control_enabled);
  ui.pushButton_actuator_steering_down->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_rtz->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_left->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_right->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_up->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_backward->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_forward->setEnabled(actuator_control_enabled);
  this->actuator_rocker->setEnabled(actuator_control_enabled);
  ui.pushButton_robot_steering_backward->setEnabled(robot_control_enabled);
  ui.pushButton_robot_steering_forward->setEnabled(robot_control_enabled);
  ui.pushButton_robot_steering_turnleft->setEnabled(robot_control_enabled);
  ui.pushButton_robot_steering_turnright->setEnabled(robot_control_enabled);
  this->robot_rocker->setEnabled(robot_control_enabled);

  ui.Slider_robot_linear_Velocity->setRange(0,1000);
  ui.spinBox_robot_linearVelocity->setRange(0,1000);
  ui.Slider_actuator_Velocity->setRange(0,780);
  ui.spinBox_actuator_Velocity->setRange(0,780);
  ui.Slider_robot_angle_Velocity->setRange(0,500);
  ui.spinBox_robot_angleVelocity->setRange(0,500);
  ui.Slider_robot_linear_Velocity->setEnabled(false);
  ui.spinBox_robot_linearVelocity->setEnabled(false);
  ui.Slider_actuator_Velocity->setEnabled(false);
  ui.spinBox_actuator_Velocity->setEnabled(false);
  ui.Slider_robot_angle_Velocity->setEnabled(false);
  ui.spinBox_robot_angleVelocity->setEnabled(false);

  ui.lineEdit_Serial_information_sendInformation->setEnabled(false);
  ui.pushButton_Serial_Port_send->setEnabled(false);

  ui.comboBox_actuator_control_mode->addItem(QString("mm"));
  ui.comboBox_actuator_control_mode->addItem(QString("cm"));
}

void MainWindow::connections()
{
  connect(ui.actionSettings, SIGNAL(triggered(bool)), this, SLOT(slot_settingsFrame()));
  connect(ui.pushbutton_quit, SIGNAL(clicked()),this, SLOT(close()));
  connect(ui.checkBox_robot_control_used, SIGNAL(stateChanged(int)),this,SLOT(slot_checkBox_robot_control_used(int)));
  connect(ui.checkBox_actuator_control_used, SIGNAL(stateChanged(int)),this,SLOT(slot_checkBox_actuator_control_used(int)));
  connect(ui.checkBox_camera_control_used,SIGNAL(stateChanged(int)),this,SLOT(slot_checkBox_camera_control_used(int)));

  connect(cmd , SIGNAL(readyReadStandardOutput()) , this , SLOT(on_readCMDOutput()));
  connect(cmd , SIGNAL(readyReadStandardError()) , this , SLOT(on_readCMDError()));

  //摇杆控件控制
  connect(robot_rocker, SIGNAL(keyNumchanged(int)),this,SLOT(slot_robot_rocker(int)));

  connect(ui.Slider_robot_linear_Velocity, SIGNAL(sliderMoved(int)),ui.spinBox_robot_linearVelocity,SLOT(setValue(int)));
  connect(ui.spinBox_robot_linearVelocity, SIGNAL(valueChanged(int)),ui.Slider_robot_linear_Velocity, SLOT(setValue(int)));
  connect(ui.Slider_robot_angle_Velocity,SIGNAL(valueChanged(int)),ui.spinBox_robot_angleVelocity,SLOT(setValue(int)));
  connect(ui.spinBox_robot_angleVelocity, SIGNAL(valueChanged(int)), ui.Slider_robot_angle_Velocity, SLOT(setValue(int)));
  connect(ui.Slider_actuator_Velocity, SIGNAL(valueChanged(int)), ui.spinBox_actuator_Velocity, SLOT(setValue(int)));
  connect(ui.spinBox_actuator_Velocity, SIGNAL(valueChanged(int)), ui.Slider_actuator_Velocity, SLOT(setValue(int)));

  //按键控制
  connect(ui.pushButton_robot_steering_backward,SIGNAL(pressed()),this,SLOT(slot_robot_pushButton_control()));
  connect(ui.pushButton_robot_steering_forward,SIGNAL(pressed()),this,SLOT(slot_robot_pushButton_control()));
  connect(ui.pushButton_robot_steering_turnleft,SIGNAL(pressed()),this,SLOT(slot_robot_pushButton_control()));
  connect(ui.pushButton_robot_steering_turnright,SIGNAL(pressed()),this,SLOT(slot_robot_pushButton_control()));

  connect(ui.pushButton_robot_steering_backward, SIGNAL(released()), this, SLOT(slot_robot_pushButton_steering_stop()));
  connect(ui.pushButton_robot_steering_forward, SIGNAL(released()), this, SLOT(slot_robot_pushButton_steering_stop()));
  connect(ui.pushButton_robot_steering_turnleft, SIGNAL(released()), this, SLOT(slot_robot_pushButton_steering_stop()));
  connect(ui.pushButton_robot_steering_turnright, SIGNAL(released()), this, SLOT(slot_robot_pushButton_steering_stop()));

  //  connect(ui.pushButton_robot_steering_backward,SIGNAL(pressed()),this,SLOT(slot_robot_pushButton_send_message_control()));
  //  connect(ui.pushButton_robot_steering_forward,SIGNAL(pressed()),this,SLOT(slot_robot_pushButton_send_message_control()));
  //  connect(ui.pushButton_robot_steering_turnleft,SIGNAL(pressed()),this,SLOT(slot_robot_pushButton_send_message_control()));
  //  connect(ui.pushButton_robot_steering_turnright,SIGNAL(pressed()),this,SLOT(slot_robot_pushButton_send_message_control()));

  connect(ui.pushButton_actuator_steering_forward, SIGNAL(pressed()), this, SLOT(slot_actuator_pushButton_control()));
  connect(ui.pushButton_actuator_steering_backward, SIGNAL(pressed()), this, SLOT(slot_actuator_pushButton_control()));
  connect(ui.pushButton_actuator_steering_left, SIGNAL(pressed()), this, SLOT(slot_actuator_pushButton_control()));
  connect(ui.pushButton_actuator_steering_right, SIGNAL(pressed()), this, SLOT(slot_actuator_pushButton_control()));
  connect(ui.pushButton_actuator_steering_up, SIGNAL(pressed()), this, SLOT(slot_actuator_pushButton_control()));
  connect(ui.pushButton_actuator_steering_down, SIGNAL(pressed()), this, SLOT(slot_actuator_pushButton_control()));
  connect(ui.pushButton_actuator_steering_rtz, SIGNAL(pressed()), this, SLOT(slot_actuator_pushButton_control()));
  //connect(ui.pushButton_robot_steering_forward, SIGNAL(clicked(bool)), this, SLOT(slot_robot_pushButton_control_MG()));
  //connect(ui.pushButton_robot_steering_forward, SIGNAL(released()), this, SLOT(slot_robot_pushButton_control_EX()));

  ui.pushButton_robot_steering_forward->setAutoRepeat(true);
  ui.pushButton_robot_steering_forward->setAutoRepeatInterval(500);
  ui.pushButton_robot_steering_backward->setAutoRepeat(true);
  ui.pushButton_robot_steering_backward->setAutoRepeatInterval(500);
  ui.pushButton_robot_steering_turnleft->setAutoRepeat(true);
  ui.pushButton_robot_steering_turnleft->setAutoRepeatInterval(500);
  ui.pushButton_robot_steering_turnright->setAutoRepeat(true);
  ui.pushButton_robot_steering_turnright->setAutoRepeatInterval(500);

  ui.pushButton_actuator_steering_forward->setAutoRepeat(true);
  ui.pushButton_actuator_steering_forward->setAutoRepeatInterval(100);
  ui.pushButton_actuator_steering_backward->setAutoRepeat(true);
  ui.pushButton_actuator_steering_backward->setAutoRepeatInterval(100);
  ui.pushButton_actuator_steering_left->setAutoRepeat(true);
  ui.pushButton_actuator_steering_left->setAutoRepeatInterval(100);
  ui.pushButton_actuator_steering_right->setAutoRepeat(true);
  ui.pushButton_actuator_steering_right->setAutoRepeatInterval(100);
  ui.pushButton_actuator_steering_up->setAutoRepeat(true);
  ui.pushButton_actuator_steering_up->setAutoRepeatInterval(100);
  ui.pushButton_actuator_steering_down->setAutoRepeat(true);
  ui.pushButton_actuator_steering_down->setAutoRepeatInterval(100);

  //serial port
  connect(ui.pushButton_Serial_Port_clear, SIGNAL(clicked(bool)), ui.listWidget_Display_Serial_Port_Information, SLOT(clear()));
  connect(ui.pushButton_Serial_Port_open, SIGNAL(clicked(bool)), this, SLOT(slot_serial_port_open_read()));
  connect(ui.pushButton_Serial_Port_send, SIGNAL(clicked(bool)), this, SLOT(slot_serial_port_send_message()));
  connect(ui.pushButton_Serial_Port_saveText, SIGNAL(clicked(bool)),this, SLOT(slot_serial_port_save_current_text()));
}

void MainWindow::initVideos()
{
  //读取有关话题订阅的默认设置
  QSettings video_topic_settings("video topic settings", "weeding_robot_gui");
  QStringList names = video_topic_settings.value("names").toStringList();
  QStringList topics = video_topic_settings.value("topics").toStringList();

  if(names.size()==4)
  {
    ui.labelimage_display_robot_surrounding->setText(names[0]);
    ui.labelimage_display_others->setText(names[1]);
    ui.labelimage_display_ZED_camera->setText(names[2]);
    ui.labelimage_display_depth_image->setText(names[3]);
  }
  if(topics.size()==4)
  {
    if(topics[0]!="")
      qnode.subImage(topics[0],1);
    if(topics[1]!="")
      qnode.subImage(topics[1],2);
    if(topics[2]!="")
      qnode.subImage(topics[2],3);
    if(topics[3]!="")
      qnode.subImage(topics[3],4);
  }
  connect(&qnode, SIGNAL(showImage(int,QImage)), this, SLOT(slot_showVideo(int,QImage)));
}

void MainWindow::initSerialPort()
{
  current_serial_port = new QSerialPort();
  timerSerial = new QTimer(this);
  connect(current_serial_port, SIGNAL(readyRead()),this,SLOT(slot_serial_port_read_message()));
  connect(timerSerial,SIGNAL(timeout()),this,SLOT(slot_serial_port_show_message()));
  foreach (const QSerialPortInfo &info, QSerialPortInfo::availablePorts())
  {
    QString serial_port_name = QString("%1").arg(info.portName());
    ui.comboBox_Serial_Port_Selection->addItem(serial_port_name);
    ui.listWidget_Display_Serial_Port_Information->addItem(QString("Name : %1").arg(serial_port_name));
    ui.listWidget_Display_Serial_Port_Information->addItem(QString("Description : %1").arg(info.description()));
    ui.listWidget_Display_Serial_Port_Information->addItem(QString("Manufacturer : %1").arg(info.manufacturer()));
    ui.listWidget_Display_Serial_Port_Information->addItem(QString("Serial Number : %1").arg(info.serialNumber()));
    ui.listWidget_Display_Serial_Port_Information->addItem(QString("System Location : %1").arg(info.systemLocation()));
    ui.listWidget_Display_Serial_Port_Information->addItem(QString("\n"));
    //ui->saerialport->addItem(info.portName());
    ui.listWidget_Display_Serial_Port_Information->scrollToBottom();
  }

  ui.comboBox_Serial_Port_BaudRate->addItem(QString("9600"));
  ui.comboBox_Serial_Port_BaudRate->addItem(QString("1200"));
  ui.comboBox_Serial_Port_BaudRate->addItem(QString("2400"));
  ui.comboBox_Serial_Port_BaudRate->addItem(QString("4800"));
  ui.comboBox_Serial_Port_BaudRate->addItem(QString("19200"));
  ui.comboBox_Serial_Port_BaudRate->addItem(QString("38400"));
  ui.comboBox_Serial_Port_BaudRate->addItem(QString("57600"));
  ui.comboBox_Serial_Port_BaudRate->addItem(QString("115200"));

  ui.comboBox_Serial_Port_DataBit->addItem(QString("8"));
  ui.comboBox_Serial_Port_DataBit->addItem(QString("5"));
  ui.comboBox_Serial_Port_DataBit->addItem(QString("6"));
  ui.comboBox_Serial_Port_DataBit->addItem(QString("7"));

  ui.comboBox_Serial_Port_ParityBit->addItem(QString("None"));
  ui.comboBox_Serial_Port_ParityBit->addItem(QString("OddParity"));
  ui.comboBox_Serial_Port_ParityBit->addItem(QString("EvenParity"));
  ui.comboBox_Serial_Port_ParityBit->addItem(QString("SpaceParity"));
  ui.comboBox_Serial_Port_StopBit->addItem(QString("1"));
  ui.comboBox_Serial_Port_StopBit->addItem(QString("2"));

  ui.comboBox_Serial_Port_Showdata_format->addItem(QString("Str"));
  ui.comboBox_Serial_Port_Showdata_format->addItem(QString("Hex"));
}

void MainWindow::serial_port_send_motion_control_message(int right_wheel_velocity,int left_wheel_velocity)
{
  if(current_serial_port->isOpen())
  {
    bool isHexS = true;
    QByteArray sdata;
    if(isHexS)
    {
      //sdata.append(send_message).toHex();
      // 履带车电机安装顺序不一样 !M v -v是前进，轮式小车 !M v v
      QString send_message = QString("!M %1 -%2").arg(right_wheel_velocity).arg(left_wheel_velocity);
      sdata.append(send_message);
      // 串口数据要加\r\n在末尾位置
      sdata.append("\r\n");
    }
    current_serial_port->write(sdata, sdata.length());
    ui.lineEdit_Serial_information_sendInformation->clear();
  }
}


/*****************************************************************************
** Implementation [Slots]
*****************************************************************************/
void MainWindow::slot_settingsFrame()
{
  qDebug() << "settings";
  if(setting != NULL)
  {
    delete setting;
    setting = new settings();
    setting->setWindowModality(Qt::ApplicationModal);
    QRect rect = this->geometry();
    int x = rect.x() + rect.width()/2 - this->width() /4;
    int y = rect.y() + rect.height()/2 - this->height()/4;
    setting->move(x,y);
    setting->show();
  }
  else
  {
    setting = new settings();
    setting->setWindowModality(Qt::ApplicationModal);
    QRect rect = this->geometry();
    int x = rect.x() + rect.width()/2 - this->width() /4;
    int y = rect.y() + rect.height()/2 - this->height()/4;
    setting->move(x,y);
    setting->show();
  }
}

void MainWindow::slot_checkBox_robot_control_used(int state)
{
  qDebug() << "checkBox state Changed " << state;
  if(state == 2)
  {
    robot_control_enabled = true;
  }
  else {
    robot_control_enabled = false;
    ui.Slider_robot_angle_Velocity->setValue(0);
    ui.Slider_robot_linear_Velocity->setValue(0);
    ui.spinBox_robot_angleVelocity->setValue(0);
    ui.spinBox_robot_linearVelocity->setValue(0);
  }
  ui.pushButton_robot_steering_backward->setEnabled(robot_control_enabled);
  ui.pushButton_robot_steering_forward->setEnabled(robot_control_enabled);
  ui.pushButton_robot_steering_turnleft->setEnabled(robot_control_enabled);
  ui.pushButton_robot_steering_turnright->setEnabled(robot_control_enabled);
  this->robot_rocker->setEnabled(robot_control_enabled);
  ui.Slider_robot_angle_Velocity->setEnabled(robot_control_enabled);
  ui.spinBox_robot_angleVelocity->setEnabled(robot_control_enabled);
  ui.spinBox_robot_linearVelocity->setEnabled(robot_control_enabled);
  ui.Slider_robot_linear_Velocity->setEnabled(robot_control_enabled);
}

void MainWindow::slot_checkBox_actuator_control_used(int state)
{
  qDebug() << "checkBox state Changed " << state;
  if(state == 2)
  {
    actuator_control_enabled = true;
  }
  else {
    actuator_control_enabled = false;
    ui.Slider_actuator_Velocity->setValue(0);
    ui.spinBox_actuator_Velocity->setValue(0);
  }
  ui.pushButton_actuator_steering_down->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_rtz->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_left->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_right->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_up->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_backward->setEnabled(actuator_control_enabled);
  ui.pushButton_actuator_steering_forward->setEnabled(actuator_control_enabled);
  this->actuator_rocker->setEnabled(actuator_control_enabled);
  ui.Slider_actuator_Velocity->setEnabled(actuator_control_enabled);
  ui.spinBox_actuator_Velocity->setEnabled(actuator_control_enabled);

}

void MainWindow::slot_checkBox_camera_control_used(int state)
{
  if(state == 2)
  {
    camera_control_enabled = true;
  }
  else
  {
    camera_control_enabled = false;
  }
  ui.pushButton_camera_upforward->setEnabled(camera_control_enabled);
  ui.pushButton_camera_downforward->setEnabled(camera_control_enabled);

}

void MainWindow::showNoMasterMessage() {
  QMessageBox msgBox;
  msgBox.setText("Couldn't find the ros master.");
  msgBox.exec();
  close();
}

/*
* @todo
*/
void MainWindow::slot_robot_rocker(int key)
{
  qDebug()<<"key: "<<key;
  //速度
  float linear_velocity = ui.Slider_robot_linear_Velocity->value()*0.01;
  float angle_velocity = ui.Slider_robot_angle_Velocity->value()*0.01;
  bool robot_control_used = false;  //这个使用来控制是否使用全向轮模式的,设置的全向轮模式有问题，这里把它停掉
  switch (key) {
  case upleft:
    qnode.move_base(robot_control_used?'U':'u',linear_velocity,angle_velocity);
    break;
  case up:
    qnode.move_base(robot_control_used?'I':'i',linear_velocity,angle_velocity);
    break;
  case upright:
    qnode.move_base(robot_control_used?'O':'o',linear_velocity,angle_velocity);
    break;
  case left:
    qnode.move_base(robot_control_used?'J':'j',linear_velocity,angle_velocity);
    break;
  case right:
    qnode.move_base(robot_control_used?'L':'l',linear_velocity,angle_velocity);
    break;
  case down:
    qnode.move_base(robot_control_used?'M':'m',linear_velocity,angle_velocity);
    break;
  case downleft:
    qnode.move_base(robot_control_used?'<':',',linear_velocity,angle_velocity);
    break;
  case downright:
    qnode.move_base(robot_control_used?'>':'.',linear_velocity,angle_velocity);
    break;
  case stop:
    qnode.move_base(robot_control_used?'K':'k', 0, 0);
  }
}

/**
 * @brief 使用串口方式控制移动平台运动
 *
 * @return void
 */
void MainWindow::slot_robot_pushButton_send_message_control()
{
  QPushButton* btn=qobject_cast<QPushButton*>(sender());
  char key=btn->text().toStdString()[0];
  bool robot_control_used = false;
  //速度
  float linear_velocity = ui.Slider_robot_linear_Velocity->value();
  float angle_velocity = ui.Slider_robot_angle_Velocity->value();
  int left_wheel_velocity, right_wheel_velocity;
  switch (key) {
  case 'u':
    qnode.move_base(robot_control_used?'U':'u',linear_velocity,angle_velocity);
    break;
  case 'i':
    left_wheel_velocity = -linear_velocity;
    right_wheel_velocity = -linear_velocity;
    MainWindow::serial_port_send_motion_control_message(right_wheel_velocity,left_wheel_velocity);
    break;
  case 'o':
    qnode.move_base(robot_control_used?'O':'o',linear_velocity,angle_velocity);
    break;
  case 'j':
    left_wheel_velocity = -linear_velocity;
    right_wheel_velocity = -linear_velocity-angle_velocity;
    MainWindow::serial_port_send_motion_control_message(right_wheel_velocity,left_wheel_velocity);
    break;
  case 'l':
    left_wheel_velocity = -linear_velocity-angle_velocity;
    right_wheel_velocity = -linear_velocity;
    MainWindow::serial_port_send_motion_control_message(right_wheel_velocity,left_wheel_velocity);
    break;
  case 'm':
    qnode.move_base(robot_control_used?'M':'m',linear_velocity,angle_velocity);
    break;
  case ',':
    left_wheel_velocity = linear_velocity;
    right_wheel_velocity = linear_velocity;
    MainWindow::serial_port_send_motion_control_message(right_wheel_velocity,left_wheel_velocity);
    break;
    break;
  case '.':
    qnode.move_base(robot_control_used?'>':'.',linear_velocity,angle_velocity);
    break;
  }
}

void MainWindow::serial_port_send_actuator_control_message(QByteArray send_message)
{
  if(current_serial_port->isOpen())
  {
    current_serial_port->write(send_message, send_message.length());
    ui.listWidget_Display_Serial_Port_Information->addItem(QString(send_message));

    g_x < 0 ? g_x = 0 : g_x = g_x;
    g_y < 0 ? g_y = 0 : g_y = g_y;
    g_z < 0 ? g_z = 0 : g_z = g_z;
    ui.listWidget_Display_Serial_Port_Information->addItem(QString("x: %1 y: %2 z: %3").arg(g_x).arg(g_y).arg(g_z));
    ui.listWidget_Display_Serial_Port_Information->scrollToBottom();
    ui.label_actuator_x_coordinate_position->setText(QString("%1").arg(g_x));
    ui.label_actuator_y_coordinate_position->setText(QString("%1").arg(g_y));
    ui.label_actuator_z_coordinate_position->setText(QString("%1").arg(g_z));
    ui.lineEdit_Serial_information_sendInformation->clear();
  }
}

int ten2hex(int data)
{
  std::vector<int> byteVec;
  int tempByte, lastByte, hexData;

  tempByte = 0;
  lastByte = 0;
  hexData = 0;
  lastByte = data % 10;
  while (data / 10) {
    tempByte = data / 10;
    byteVec.push_back(tempByte);
    data /= 10;
  }
  for(int i=byteVec.size();i > 0;i--)
  {
    hexData += byteVec[i-1] * std::pow(16, i);
  }
  hexData += lastByte;
  return hexData;
}

/**
 * @brief 执行机构控制按钮回调函数，主要是包括前进、后退、左移、右移、上移和下移
 * @attention 指令介绍：0x50开头 + 0x50 归零、0x51指定坐标、0x52点动 + 0x00 x正方向、0x01 x负方向
 *                      + 0xxx 位移xx + 0x00 y左方向、0x01 y右方向 + 0xxx 位移xx + 0x00 z下方向、0x01 z上方向 + 0x60 结束标志位
 *
 * @return void 回调函数无返回值
 *     -<em>false</em> fail
 *     -<em>true</em> succeed
 */
void MainWindow::slot_actuator_pushButton_control()
{
  QPushButton* btn=qobject_cast<QPushButton*>(sender());
  char key=btn->text().toStdString()[0];
  //速度
  int velocity = ui.Slider_actuator_Velocity->value();
  int mode = 0;

  if(ui.comboBox_actuator_control_mode->currentText() == QString("mm"))
  {
    mode = ten2hex(52);
  }
  else if(ui.comboBox_actuator_control_mode->currentText() == QString("cm"))
  {
    mode = ten2hex(52);
  }

  /* 十进制速度转换为16进制速度 */
  /* ten2hex 函数转换之后是发送10mm就是直接发0x10,不加ten2hex函数就是发送10mm就是发送0x0A */
  //int velocity_16_high = ten2hex(velocity / 100);
  //int velocity_16_low = ten2hex(velocity % 100);
  int velocity_16_high = velocity / 100;
  int velocity_16_low = velocity % 100;

  QByteArray send_message;
  send_message.resize(12);
  switch (key) {
  case 'f':
    g_x += velocity;
    std::cout << velocity << std::endl;
    send_message[0] = 0x50;
    send_message[1] = mode;
    send_message[2] = 0x00;
    send_message[3] = velocity_16_high;
    send_message[4] = velocity_16_low;
    send_message[5] = 0x00;
    send_message[6] = 0x00;
    send_message[7] = 0x00;
    send_message[8] = 0x00;
    send_message[9] = 0x00;
    send_message[10] = 0x00;
    send_message[11] = 0x60;
    serial_port_send_actuator_control_message(send_message);
    break;
  case 'b':
    g_x -= velocity;
    send_message[0] = 0x50;
    send_message[1] = mode;
    send_message[2] = 0x01;
    send_message[3] = velocity_16_high;
    send_message[4] = velocity_16_low;
    send_message[5] = 0x00;
    send_message[6] = 0x00;
    send_message[7] = 0x00;
    send_message[8] = 0x00;
    send_message[9] = 0x00;
    send_message[10] = 0x00;
    send_message[11] = 0x60;
    serial_port_send_actuator_control_message(send_message);
    break;
  case 'l':
    g_y += velocity;
    send_message[0] = 0x50;
    send_message[1] = mode;
    send_message[2] = 0x00;
    send_message[3] = 0x00;
    send_message[4] = 0x00;
    send_message[5] = 0x00;
    send_message[6] = velocity_16_high;
    send_message[7] = velocity_16_low;
    send_message[8] = 0x00;
    send_message[9] = 0x00;
    send_message[10] = 0x00;
    send_message[11] = 0x60;
    serial_port_send_actuator_control_message(send_message);
    break;
  case 'r':
    g_y -= velocity;
    send_message[0] = 0x50;
    send_message[1] = mode;
    send_message[2] = 0x00;
    send_message[3] = 0x00;
    send_message[4] = 0x00;
    send_message[5] = 0x01;
    send_message[6] = velocity_16_high;
    send_message[7] = velocity_16_low;
    send_message[8] = 0x00;
    send_message[9] = 0x00;
    send_message[10] = 0x00;
    send_message[11] = 0x60;
    serial_port_send_actuator_control_message(send_message);
    break;
  case 'u':
    g_z -= velocity;
    send_message[0] = 0x50;
    send_message[1] = mode;
    send_message[2] = 0x00;
    send_message[3] = 0x00;
    send_message[4] = 0x00;
    send_message[5] = 0x00;
    send_message[6] = 0x00;
    send_message[7] = 0x00;
    send_message[8] = 0x01;
    send_message[9] = velocity_16_high;
    send_message[10] = velocity_16_low;
    send_message[11] = 0x60;
    serial_port_send_actuator_control_message(send_message);
    break;
  case 'd':
    g_z += velocity;
    send_message[0] = 0x50;
    send_message[1] = mode;
    send_message[2] = 0x00;
    send_message[3] = 0x00;
    send_message[4] = 0x00;
    send_message[5] = 0x00;
    send_message[6] = 0x00;
    send_message[7] = 0x00;
    send_message[8] = 0x00;
    send_message[9] = velocity_16_high;
    send_message[10] = velocity_16_low;
    send_message[11] = 0x60;
    serial_port_send_actuator_control_message(send_message);
    break;
  case '0':
    g_x = 0;
    g_y = 0;
    g_z = 0;
    send_message[0] = 0x50;
    send_message[1] = 0x50;
    send_message[2] = 0x00;
    send_message[3] = 0x00;
    send_message[4] = 0x00;
    send_message[5] = 0x00;
    send_message[6] = 0x00;
    send_message[7] = 0x00;
    send_message[8] = 0x00;
    send_message[9] = 0x00;
    send_message[10] = 0x00;
    send_message[11] = 0x60;
    serial_port_send_actuator_control_message(send_message);
    break;
  }
}

void MainWindow::slot_robot_pushButton_control()
{
  QPushButton* btn=qobject_cast<QPushButton*>(sender());
  char key=btn->text().toStdString()[0];
  //速度
  float linear_velocity = ui.Slider_robot_linear_Velocity->value()*0.01;
  float angle_velocity = ui.Slider_robot_angle_Velocity->value()*0.01;
  bool robot_control_used = false;  //这个使用来控制是否使用全向轮模式的,设置的全向轮模式有问题，这里把它停掉
  switch (key) {
  case 'u':
    qnode.move_base(robot_control_used?'U':'u',linear_velocity,angle_velocity);
    break;
  case 'i':
    qnode.move_base(robot_control_used?'I':'i',linear_velocity,angle_velocity);
    break;
  case 'o':
    qnode.move_base(robot_control_used?'O':'o',linear_velocity,angle_velocity);
    break;
  case 'j':
    qnode.move_base(robot_control_used?'J':'j',linear_velocity,angle_velocity);
    break;
  case 'l':
    qnode.move_base(robot_control_used?'L':'l',linear_velocity,angle_velocity);
    break;
  case 'm':
    qnode.move_base(robot_control_used?'M':'m',linear_velocity,angle_velocity);
    break;
  case ',':
    qnode.move_base(robot_control_used?'<':',',linear_velocity,angle_velocity);
    break;
  case '.':
    qnode.move_base(robot_control_used?'>':'.',linear_velocity,angle_velocity);
    break;
  }
}

/**
 * @brief 检查串口状态，设置按钮使能状态和串口参数
 *
 * @return void
 */
void MainWindow::slot_serial_port_open_read()
{
  bool is_serial_port_opened = current_serial_port->isOpen();
  if(!is_serial_port_opened){
    /* 设置移动平台和执行机构按钮不使能，如果使用ROS控制注释掉这两句 */
    //    ui.checkBox_robot_control_used->setEnabled(true);
    //    ui.checkBox_actuator_control_used->setEnabled(true);

    current_serial_port->setPortName(ui.comboBox_Serial_Port_Selection->currentText());
    int baud = ui.comboBox_Serial_Port_BaudRate->currentText().toInt();
    int databits = ui.comboBox_Serial_Port_DataBit->currentText().toInt();
    int stopbits = ui.comboBox_Serial_Port_StopBit->currentText().toInt();
    QString paritybits = ui.comboBox_Serial_Port_ParityBit->currentText();
    int encode_paritybits;
    if(paritybits == QString("None"))
    {
      encode_paritybits = 0;
    }
    else if (paritybits == QString("OddParity")) {
      encode_paritybits == 1;
    }
    else if (paritybits == QString("EvenParity")) {
      encode_paritybits == 2;
    }
    else {
      encode_paritybits == 3;
    }
    switch (baud) {
    case 1200:
      current_serial_port->setBaudRate(QSerialPort::Baud1200,QSerialPort::AllDirections);
      break;
    case 2400:
      current_serial_port->setBaudRate(QSerialPort::Baud2400,QSerialPort::AllDirections);
      break;
    case 4800:
      current_serial_port->setBaudRate(QSerialPort::Baud4800,QSerialPort::AllDirections);
      break;
    case 9600:
      current_serial_port->setBaudRate(QSerialPort::Baud9600,QSerialPort::AllDirections);
      break;
    case 19200:
      current_serial_port->setBaudRate(QSerialPort::Baud19200,QSerialPort::AllDirections);
      break;
    case 38400:
      current_serial_port->setBaudRate(QSerialPort::Baud38400,QSerialPort::AllDirections);
      break;
    case 57600:
      current_serial_port->setBaudRate(QSerialPort::Baud57600,QSerialPort::AllDirections);
      break;
    case 115200:
      current_serial_port->setBaudRate(QSerialPort::Baud115200,QSerialPort::AllDirections);
      break;
    default:
      break;
    }

    switch (databits) {
    case 5:
      current_serial_port->setDataBits(QSerialPort::Data5);
      break;
    case 6:
      current_serial_port->setDataBits(QSerialPort::Data6);
      break;
    case 7:
      current_serial_port->setDataBits(QSerialPort::Data7);
      break;
    case 8:
      current_serial_port->setDataBits(QSerialPort::Data8);
      break;
    default:
      break;
    }

    switch (encode_paritybits) {
    case 0:
      current_serial_port->setParity(QSerialPort::NoParity);
      break;
    case 1:
      current_serial_port->setParity(QSerialPort::OddParity);
      break;
    case 2:
      current_serial_port->setParity(QSerialPort::EvenParity);
      break;
    case 3:
      current_serial_port->setParity(QSerialPort::SpaceParity);
      break;
    default:
      break;
    }

    switch (stopbits) {
    case 1:
      current_serial_port->setStopBits(QSerialPort::OneStop);
      break;
    case 2:
      current_serial_port->setStopBits(QSerialPort::TwoStop);
      break;
    default:
      break;
    }
    current_serial_port->setFlowControl(QSerialPort::SoftwareControl);
    is_serial_port_opened = current_serial_port->open(QIODevice::ReadWrite);//打开串口并选择读写模式
    if(is_serial_port_opened)
    {
      ui.listWidget_Display_Serial_Port_Information->addItem(QString("打开串口成功"));
      ui.listWidget_Display_Serial_Port_Information->scrollToBottom();
    }

    else
    {
      ui.listWidget_Display_Serial_Port_Information->addItem(QString("打开串口失败"));
      ui.listWidget_Display_Serial_Port_Information->scrollToBottom();
      return;
    }

    ui.lineEdit_Serial_information_sendInformation->setEnabled(true);
    ui.pushButton_Serial_Port_send->setEnabled(true);
    ui.pushButton_Serial_Port_open->setText(QString("Close"));
  }
  else{
    /* 设置移动平台和执行机构按钮不使能，如果使用ROS控制注释掉这两句 */
    //    ui.checkBox_robot_control_used->setEnabled(false);
    //    ui.checkBox_actuator_control_used->setEnabled(false);

    current_serial_port->close();
    ui.listWidget_Display_Serial_Port_Information->addItem(QString("串口关闭"));
    ui.listWidget_Display_Serial_Port_Information->scrollToBottom();
    ui.pushButton_Serial_Port_open->setText(QString("Open"));
    ui.pushButton_Serial_Port_send->setEnabled(false);
    ui.lineEdit_Serial_information_sendInformation->setEnabled(false);
  }
}

void MainWindow::slot_serial_port_show_message()
{
  timerSerial->stop();
  QByteArray data = buffer;//读取缓冲区数据
  showdata.append(data);
  QString show = "";
  bool isHexR;
  if(ui.comboBox_Serial_Port_Showdata_format->currentText() == QString("Hex"))
  {
    isHexR = true;
  }
  else {
    isHexR = false;
  }

  if(isHexR)
  {
    for (int i = 0; i < showdata.length(); i++)
    {
      qint8 outChar = showdata[i];
      QString str = QString("%1").arg(outChar&0xFF, 2, 16, QLatin1Char('0'))+" ";
      show+= str.toUpper();
    }

  }else
  {
    show+=QString(showdata);
  }
  qDebug() << show;
  ui.listWidget_Display_Serial_Port_Information->addItem(show);
  ui.listWidget_Display_Serial_Port_Information->scrollToBottom();
  showdata.clear();
  buffer.clear();
}

void MainWindow::slot_serial_port_read_message()
{
  timerSerial->start(100);
  //current_serial_port->waitForReadyRead(10);
  buffer.append(current_serial_port->readAll());//将读到的数据放入缓冲区
  //showdata.append(current_serial_port->readAll());
}

void MainWindow::slot_serial_port_save_current_text()
{
  qDebug() << "write data";
  QStringList lines;
  QString line;
  int row=0;
  QFile file("/home/chen/serial_port.txt");//打开该文件进入编辑模式
  if(file.open(QIODevice::Append))//如果被打开
  {
    file.resize("/home/chen/serial_port.txt",0);//清空内容
    while(row<(ui.listWidget_Display_Serial_Port_Information->count()))//从wistlidget里一行一行的读取文本并存入lines里边
    {line=ui.listWidget_Display_Serial_Port_Information->item(row)->text();
      row++;
      lines<<line;
    }
    QTextStream stream( &file );//开始写入文本
    for ( QStringList::Iterator it=lines.begin();it!=lines.end();++it )
      stream<<*it<<"\r\n";//注意是"\r\n"！！！！！！如果是单个"\n"  保存文本的时候它不保留换行符！！！！
    file.close();
  }
  else{
    qDebug() << "Open Text Failed";
  }
}

void MainWindow::slot_robot_pushButton_control_EX()
{
  if(current_serial_port->isOpen())
  {
    bool isHexS = true;
    QByteArray sdata;
    if(isHexS)
    {
      //sdata.append(send_message).toHex();
      QString send_message = QString("!EX");
      sdata.append(send_message);
      sdata.append("\r\n");
    }
    current_serial_port->write(sdata, sdata.length());
    ui.lineEdit_Serial_information_sendInformation->clear();
  }
}

void MainWindow::slot_robot_pushButton_control_MG()
{
  if(current_serial_port->isOpen())
  {
    bool isHexS = true;
    QByteArray sdata;
    if(isHexS)
    {
      //sdata.append(send_message).toHex();
      QString send_message = QString("!MG");
      sdata.append(send_message);
      sdata.append("\r\n");
    }
    current_serial_port->write(sdata, sdata.length());
    ui.lineEdit_Serial_information_sendInformation->clear();
  }
}

void MainWindow::slot_serial_port_send_message()
{
  if(current_serial_port->isOpen())
  {
    QString send_message = ui.lineEdit_Serial_information_sendInformation->text();
    bool isHexS = true;
    QByteArray sdata;
    if(isHexS)
    {
      //sdata.append(send_message).toHex();

      sdata.append(send_message);
      sdata.append("\r\n");

    }
    else
    {
      sdata.append(send_message);
    }
    current_serial_port->write(sdata, sdata.length());
    ui.lineEdit_Serial_information_sendInformation->clear();
  }
}

void MainWindow::on_button_connect_clicked(bool check ) {
  if ( ui.checkbox_use_environment->isChecked() ) {
    if ( !qnode.init() ) {
      is_ros_connected = false;
      ui.checkBox_robot_control_used->setEnabled(false);
      ui.checkBox_actuator_control_used->setEnabled(false);
      ui.checkBox_camera_control_used->setEnabled(false);
      ui.pushButton_roscore->setEnabled(true);
      showNoMasterMessage();

    } else {
      is_ros_connected = true;
      ui.checkBox_actuator_control_used->setEnabled(true);
      ui.checkBox_camera_control_used->setEnabled(true);
      ui.checkBox_robot_control_used->setEnabled(true);
      ui.button_connect->setEnabled(false);
      ui.pushButton_roscore->setEnabled(false);
      initVideos();
    }
  } else {
    if ( ! qnode.init(ui.line_edit_master->text().toStdString(),
                      ui.line_edit_host->text().toStdString()) ) {
      showNoMasterMessage();
      is_ros_connected = false;
      ui.checkBox_robot_control_used->setEnabled(false);
      ui.checkBox_actuator_control_used->setEnabled(false);
      ui.checkBox_camera_control_used->setEnabled(false);
      ui.pushButton_roscore->setEnabled(true);
    } else {
      is_ros_connected = true;
      ui.checkBox_robot_control_used->setEnabled(true);
      ui.checkBox_actuator_control_used->setEnabled(true);
      ui.checkBox_camera_control_used->setEnabled(true);
      ui.button_connect->setEnabled(false);
      ui.pushButton_roscore->setEnabled(false);
      ui.line_edit_master->setReadOnly(true);
      ui.line_edit_host->setReadOnly(true);
      ui.line_edit_topic->setReadOnly(true);
      initVideos();
    }
  }
}

void weeding_robot_gui::MainWindow::slot_robot_pushButton_steering_stop()
{
  QPushButton* btn=qobject_cast<QPushButton*>(sender());
  char key=btn->text().toStdString()[0];
  //速度
  float linear_velocity = 0;
  float angle_velocity = 0;
  bool robot_control_used = false;  //这个使用来控制是否使用全向轮模式的,设置的全向轮模式有问题，这里把它停掉
  switch (key) {
  case 'u':
    qnode.move_base(robot_control_used?'U':'u',linear_velocity,angle_velocity);
    break;
  case 'i':
    qnode.move_base(robot_control_used?'I':'i',linear_velocity,angle_velocity);
    break;
  case 'o':
    qnode.move_base(robot_control_used?'O':'o',linear_velocity,angle_velocity);
    break;
  case 'j':
    qnode.move_base(robot_control_used?'J':'j',linear_velocity,angle_velocity);
    break;
  case 'l':
    qnode.move_base(robot_control_used?'L':'l',linear_velocity,angle_velocity);
    break;
  case 'm':
    qnode.move_base(robot_control_used?'M':'m',linear_velocity,angle_velocity);
    break;
  case ',':
    qnode.move_base(robot_control_used?'<':',',linear_velocity,angle_velocity);
    break;
  case '.':
    qnode.move_base(robot_control_used?'>':'.',linear_velocity,angle_velocity);
    break;
  }
}

void MainWindow::on_checkbox_use_environment_stateChanged(int state) {
  bool enabled;
  if ( state == 0 ) {
    enabled = true;
  } else {
    enabled = false;
  }
  ui.line_edit_master->setEnabled(enabled);
  ui.line_edit_host->setEnabled(enabled);
  //ui.line_edit_topic->setEnabled(enabled);
}

/*****************************************************************************
** Implemenation [Slots][manually connected]
*****************************************************************************/

/**
 * This function is signalled by the underlying model. When the model changes,
 * this will drop the cursor down to the last line in the QListview to ensure
 * the user can always see the latest log message.
 */
void MainWindow::updateLoggingView() {
  ui.listView_Logging->scrollToBottom();
}

/*****************************************************************************
** Implemenation [Slots][my Slots]
*****************************************************************************/
//图像显示槽函数
void MainWindow::slot_showVideo(int frame_id, QImage image)
{
  switch (frame_id)
  {
  case 1:
    ui.labelimage_display_robot_surrounding->setPixmap(QPixmap::fromImage(image).scaled(ui.labelimage_display_robot_surrounding->width(),ui.labelimage_display_robot_surrounding->height()));
    break;
  case 2:
    ui.labelimage_display_others->setPixmap(QPixmap::fromImage(image).scaled(ui.labelimage_display_others->width(),ui.labelimage_display_others->height()));
    break;
  case 3:
    ui.labelimage_display_ZED_camera->setPixmap(QPixmap::fromImage(image).scaled(ui.labelimage_display_ZED_camera->width(),ui.labelimage_display_ZED_camera->height()));
    break;
  case 4:
    ui.labelimage_display_depth_image->setPixmap(QPixmap::fromImage(image).scaled(ui.labelimage_display_depth_image->width(),ui.labelimage_display_depth_image->height()));
    break;
  }
}

/*****************************************************************************
** Implementation [Menu]
*****************************************************************************/

void MainWindow::on_actionAbout_triggered() {
  QMessageBox::about(this, tr("About and Copyright"),tr("<h2>Weeding Robot Control Interface 0.10</h2><p>Copyright Weeding Robot</p><p>This package is a weeding robot control interface.</p>"));
}

/*****************************************************************************
** Implementation [Configuration]
*****************************************************************************/

void MainWindow::ReadSettings() {
  QSettings settings("Qt-Ros Package", "weeding_robot_gui");
  restoreGeometry(settings.value("geometry").toByteArray());
  restoreState(settings.value("windowState").toByteArray());
  master_url = settings.value("master_url",QString("http://192.168.1.2:11311/")).toString();
  host_url = settings.value("host_url", QString("192.168.1.3")).toString();
  use_enviroment_variables = settings.value("use_environment_variables", false).toBool();
  remember_settings = settings.value("remember_settings", bool(true)).toBool();
  //QString topic_name = settings.value("topic_name", QString("/chatter")).toString();
  ui.line_edit_master->setText(master_url);
  ui.line_edit_host->setText(host_url);
  //ui.line_edit_topic->setText(topic_name);
  bool remember = settings.value("remember_settings", false).toBool();
  ui.checkbox_remember_settings->setChecked(remember);
  ui.checkbox_use_environment->setChecked(remember_settings);
  if ( remember_settings ) {
    ui.line_edit_master->setEnabled(false);
    ui.line_edit_host->setEnabled(false);
    //ui.line_edit_topic->setEnabled(false);
  }
}

void MainWindow::WriteSettings() {
  QSettings settings("Qt-Ros Package", "weeding_robot_gui");
  settings.setValue("master_url",ui.line_edit_master->text());
  settings.setValue("host_url",ui.line_edit_host->text());
  //settings.setValue("topic_name",ui.line_edit_topic->text());
  settings.setValue("use_environment_variables",QVariant(ui.checkbox_use_environment->isChecked()));
  settings.setValue("remember_settings", QVariant(ui.checkbox_remember_settings->isChecked()));
  settings.setValue("geometry", saveGeometry());
  settings.setValue("windowState", saveState());
  settings.setValue("remember_settings",QVariant(ui.checkbox_remember_settings->isChecked()));

}

void MainWindow::closeEvent(QCloseEvent *event)
{
  WriteSettings();
  QMainWindow::closeEvent(event);
}

}  // namespace weeding_robot_gui


void weeding_robot_gui::MainWindow::on_pushButton_roscore_clicked()
{
  system("gnome-terminal -- bash -c 'source /opt/ros/kinetic/setup.bash ;roscore'&");
  ui.pushButton_roscore->setEnabled(false);
}

void weeding_robot_gui::MainWindow::on_readCMDError()
{
  ui.textEdit_CMDLogging->append(cmd->readAllStandardError().data());
}

void weeding_robot_gui::MainWindow::on_readCMDOutput()
{
  ui.textEdit_CMDLogging->append(cmd->readAllStandardOutput().data());
}

void weeding_robot_gui::MainWindow::on_pushButton_CMDSend_clicked()
{
  QString text = ui.lineEdit_CMD->text();
  cmd->write(text.toUtf8() + "\n"); //向终端写入“ls”命令，注意尾部的“\n”不可省略
}

void weeding_robot_gui::MainWindow::on_pushButton_OpenCamera_clicked()
{
  cmd->write("lsusb | grep 2b03 \n");
  system("gnome-terminal -- bash -c 'source /home/chen/Packages/catkin_ws_ZED-ROS/devel/setup.bash;roslaunch zed_wrapper zed2.launch'&");
  //    if(cmd->readAllStandardOutput().size() > 0)
  //    {
  //      system("gnome-terminal -- bash -c 'source /opt/ros/kinetic/setup.bash ;roscore'&");
  //    }
  //    else
  //    {
  //      QMessageBox::information(0, "Error", "ZED Camera not found");
  //    }
}
